%% 四架无人机
clc; clear; 
close all;
format long;

%% parameters
global dt Quadrotor_Num
global Tf
quadrotor_parameters;
t = 0 : dt : Tf;
N = Tf/dt + 1;
% UAV's flag
flag = zeros(Quadrotor_Num,1); 
for Num = 1:Quadrotor_Num
    flag(Num) = Num;
end

%% Initialization Position1
x0 = zeros(12,Quadrotor_Num);
x0(:,1) = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];  % Leader
x0(:,2) = [0; 0; 0; 0; 0; 0; 3; 0; 2; 0; 0; 0];  % Follower1
x0(:,3) = [0; 0; 0; 0; 0; 0; 3; 0; -2; 0; 0; 0]; % Follower2
x0(:,4) = [0; 0; 0; 0; 0; 0; -2; 0; 3; 0; 0; 0]; % Follower3
x0(:,5) = [0; 0; 0; 0; 0; 0; -2; 0; 5; 0; 0; 0]; % Follower4
x0(:,6) = [0; 0; 0; 0; 0; 0; 2; 0; 7; 0; 0; 0];  % Follower5
x = x0;

Switch_index = 1;
%% Save_data
% Polt data
X_save = zeros(N,12,Quadrotor_Num); % States
U_Save = zeros(N,4,Quadrotor_Num);  % Controllers
Pos_errors = zeros(3,N);            % Positions errors
distances_Save = zeros(Quadrotor_Num,Quadrotor_Num,N);

Uxy = zeros(2,Quadrotor_Num,N);
Uxy1 = zeros(2,Quadrotor_Num,N);
Uxy2 = zeros(2,Quadrotor_Num);

h0_Pos = zeros(3,Quadrotor_Num);
switched_values = zeros(1, N);

%%  adjacency_Matrix + Leader
% 生成1的概率
probabilityOfOne = 1;
% 使用循环生成多个邻接矩阵
adjacency_Matrix = cell(1, Quadrotor_Num);
for Num = 1:Quadrotor_Num
    % 生成随机邻接矩阵
    adjacency_Matrix{Num} = rand(Quadrotor_Num) < probabilityOfOne;
    % 将对角线元素设置为0，以确保没有自环
    adjacency_Matrix{Num}(logical(eye(Quadrotor_Num))) = 0;
end

Leader = zeros(1,Quadrotor_Num);
Leader(1) = 1;

for i = 1:N
    %% 拓朴变换
    if mod(i,100) == 0 % 每迭代100次进行一次拓扑变换
        % 随机索引
        Switch_index = randi(numel(adjacency_Matrix));
         % 使用随机选择的 adjacency_Matrix
        current_adjacency_Matrix = adjacency_Matrix{Switch_index};
        adjacency_Matrix_new = current_adjacency_Matrix + eye(Quadrotor_Num);
    else
    adjacency_Matrix_new = adjacency_Matrix{Switch_index} + eye(Quadrotor_Num);
    end
    % 存放每次变换的值
    switched_values(1,i) = Switch_index;
    %% Desire of Position
    % 拓扑图Desire
    Xd = t(i);
    Yd = t(i);
    Zd = 0.5*t(i);
    Xd_dot = 1;
    Yd_dot = 1;
    Zd_dot = 0.5;
    Xd_2dot = 0;
    Yd_2dot = 0;
    Zd_2dot = 0;

    Des_Pos = [Xd,     Xd_dot,     Xd_2dot;
               Yd,     Yd_dot,     Yd_2dot;
               Zd,     Zd_dot,     Zd_2dot];

    %% Desire of PSI
    w = 0.1; a = deg2rad (1);
    PSId   = a*sin(w*t(i));
    PSId_dot   = a*w*cos(w*t(i));
    PSId_2dot   = -a*w*w*sin(w*t(i));
    Des_PSI = [PSId;
               PSId_dot;
               PSId_2dot];
    %% 四架无人机拓扑实时变换
    w1 = 0.02; w2 = 0.05; r1 = 10; r2 = 16;
    for Num = 1:Quadrotor_Num
        h0_Pos(:,Num) = [r1*sin(w1*t(i) + (Num - 1)*pi/2);
                         r2*sin(w2*t(i) + (Num - 1)*pi/2);
                         0];
    end

    % 每一行获取UAV间的关系
    ones_positions = cellfun(@(row) find(row > 0), num2cell(adjacency_Matrix_new, 2), 'UniformOutput', false);
    
    % 初始化存储矩阵的 cell 数组
    num_matrices = size(adjacency_Matrix_new, 1);
    x_New = cell(1, num_matrices);

    % 根据每一行＞0的位置，创建对应列的矩阵并存储到 cell 数组中
    for Num = 1:num_matrices
        column_indices = ones_positions{Num};
        x_New{Num} = zeros(13, Quadrotor_Num); % 创建13*Quadrotor_Num矩阵，其中第一行存放天线，标记为1
        x_New{Num}(1,column_indices) = 1;
        x_New{Num}(2:13, column_indices) = x(:, column_indices);
    end

    fileNames_Controller = cell(1,Quadrotor_Num);
    fileNames_Plant = cell(1,Quadrotor_Num);
    for Num = 1:Quadrotor_Num
        fileNames_Controller{Num} = sprintf('quadrotor_controller%d', Num);
        fileNames_Plant{Num} = sprintf('quadrotor_plant%d', Num);
    end
    U = zeros(4,length(fileNames_Controller));
    U_Use = zeros(4,length(fileNames_Controller)); % 用来计算alpha_dot(但仿真失败)
    k1 = zeros(12,Quadrotor_Num); k2 = zeros(12,Quadrotor_Num); k3 = zeros(12,Quadrotor_Num); k4 = zeros(12,Quadrotor_Num);
    for Plant_Num = 1:length(fileNames_Controller)
        [U(:,Plant_Num),Uxy(:,Plant_Num,i+1),Pos_errors(:,i),distances] = feval(fileNames_Controller{Plant_Num},x_New{Plant_Num},Uxy(:,Plant_Num,i), ...
                                                                      Uxy2(:,Plant_Num), h0_Pos, Leader(Plant_Num),Des_Pos,Des_PSI,flag(Plant_Num),U_Use);
        % 用来调整最大距离设置adjacency_Matrix
        distances_Save(:,Plant_Num,i) = distances;
        Uxy2(:,Plant_Num) = Uxy(:,Plant_Num,i);
        k1(:,Plant_Num) = feval(fileNames_Plant{Plant_Num},x(:,Plant_Num), U(:,Plant_Num));
        k2(:,Plant_Num) = feval(fileNames_Plant{Plant_Num},(x(:,Plant_Num) + dt/2*k1(:,Plant_Num)), U(:,Plant_Num));
        k3(:,Plant_Num) = feval(fileNames_Plant{Plant_Num},(x(:,Plant_Num) + dt/2*k2(:,Plant_Num)), U(:,Plant_Num));
        k4(:,Plant_Num) = feval(fileNames_Plant{Plant_Num},(x(:,Plant_Num) + dt*k3(:,Plant_Num)),   U(:,Plant_Num));
        x(:,Plant_Num) = x(:,Plant_Num) + dt/6*(k1(:,Plant_Num) + 2*k2(:,Plant_Num) + 2*k3(:,Plant_Num) + k4(:,Plant_Num));
        X_save(i,:,Plant_Num) = x(:,Plant_Num)';
        U_Use = U;
        U_Save(i,:,Plant_Num) = U(:,Plant_Num); 
    end
end

%% 绘制单个
%% Plot Position 
figure
quatrotor_plot(t, X_save(:,7,1)); 
hold on
quatrotor_plot(t, X_save(:,9,1));
quatrotor_plot(t, X_save(:,11,1))
grid on;
xlabel('t/s');
ylabel('位置');
legend('X','Y','Z');
%% plot error
figure
quatrotor_plot(t,Pos_errors(1,:));
xlabel('t/s');
ylabel('error/m');
legend('X');
grid on;
figure
quatrotor_plot(t,Pos_errors(2,:));
xlabel('t/s');
ylabel('error/m');
legend('Y');
grid on;
figure
quatrotor_plot(t,Pos_errors(3,:));
xlabel('t/s');
ylabel('error/m');
legend('Z');
grid on;

%% plot Attitude
figure
quatrotor_plot(t, rad2deg(X_save(:,1,1))); hold on;
legend('\phi');
xlabel('t/s');
ylabel('\phi rad/s');

figure
quatrotor_plot(t, rad2deg(X_save(:,3,1))); grid on;
legend('\theta');
xlabel('t/s');
ylabel('\theta rad/s');

figure
quatrotor_plot(t, rad2deg(X_save(:,5,1)));
xlabel('t/s');
ylabel('\psi rad/s');
legend('\psi'); 

%% plot Input
figure
quatrotor_plot(t,U_Save(:,1,1));
hold on;
legend('U1');
xlabel('t/s');
ylim([-30,30]);
grid on;

figure
quatrotor_plot(t,U_Save(:,2,1));
hold on;
legend('U2');
xlabel('t/s');
ylim([-2,5]);

figure
quatrotor_plot(t,U_Save(:,3,1));
hold on;
legend('U3');
xlabel('t/s');
ylim([-1,1]);

figure
quatrotor_plot(t,U_Save(:,4,1));
legend('U4');
xlabel('t/s');
hold on;
grid on;

%% plot adjacency_Matrix
figure
quatrotor_plot(t,switched_values(1,:));
xlabel('t/s');
hold on;
grid on;
%% 3-D Space
% steps
stepping = 500; 
% Linestyles
marker_shapes = cell(1,Quadrotor_Num);
for Num = 1:Quadrotor_Num
    shapes = {'^', 'd', 's', 'o', '+', '*', 'x'};
    marker_shapes{Num} = shapes{mod(Num - 1, length(shapes)) + 1};
end

figure
for Num = 1:Quadrotor_Num
    plot3(X_save(1:stepping:N, 7, Num), X_save(1:stepping:N, 9, Num), X_save(1:stepping:N, 11, Num), marker_shapes{Num}, 'Linewidth', 2);
    hold on;
end
for Num = 1:Quadrotor_Num
    plot3(X_save(1:stepping:N, 7, Num), X_save(1:stepping:N, 9, Num), X_save(1:stepping:N, 11, Num),'Linewidth', 2);
    hold on;
end
pointNUM = 1:stepping:N;
for Num = pointNUM
     %% Four UAVS
     for j = 1:Quadrotor_Num
        plot3([X_save(Num,7,j),X_save(Num,7,mod(j,Quadrotor_Num) + 1)], ...
              [X_save(Num,9,j),X_save(Num,9,mod(j,Quadrotor_Num) + 1)], ...
              [X_save(Num,11,j),X_save(Num,11,mod(j,Quadrotor_Num) + 1)], 'Linewidth', 1.5);
        hold on
     end
end

Xd = t; % 预期的 X 轨迹
Yd = t; % 预期的 Y 轨迹
Zd = 0.5 * t; % 预期的 Z 轨迹
% 在原图上画出预期轨迹
plot3(Xd, Yd, Zd, 'k--', 'LineWidth', 2); % 使用黑色虚线表示预期轨迹

legend({'UAV1', 'UAV2', 'UAV3', 'UAV4', 'Expected Trajectory'});
xlabel('X-横轴/m');
ylabel('Y-纵轴/m');
zlabel('Z-高度/m');
hold on;
grid on;